// 左小臂

#include <ros/ros.h> 
#include <tf/tf.h>
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <std_msgs/Int32MultiArray.h>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <string>
#include <vector>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <sstream>
#include <tf/transform_datatypes.h>

using namespace std;

serial::Serial ser; //声明串口对象
geometry_msgs::Point S1_pos;
geometry_msgs::Point L1_pos;
float L_radius = 0.04;
float S_radius = 0.03;

int Is_valid(string data)
{
    int i;
    for(i = 0; i < data.length(); i++){
        if(data[i] == '0' || data[i] == '1'|| data[i] == '2'|| data[i] == '3'|| data[i] == '4'|| data[i] == '5'|| data[i] == '6'|| data[i] == '7'|| data[i] == '8'|| data[i] == '9'|| data[i] == ' ')
        {
            continue;
        }
        else 
            return 0;
    }
    return 1;
}

int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "tactile_sensor_node2"); 
    //声明节点句柄 
    ros::NodeHandle nh; 

    ros::Publisher read_pub = nh.advertise<geometry_msgs::WrenchStamped>("/force_torque_topic3", 1000); 
    ros::Publisher read_data_pub = nh.advertise<std_msgs::Int32MultiArray>("force_data_topic3", 1000);
    //ros::Publisher read_force = nh.advertise<geometry_msgs::WrenchStamped>("force_data_topic",1000);
    //ros::Publisher marker_pub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1);

    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB1"); //AAAAAA
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port ttyUSB1"); 
        return -1; 
    } 

    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port ttyUSB2 initialized"); 
    } 
    else 
    { 
        return -1; 
    } 


    //指定循环的频率 
    float f =0.0;
    ros::Rate loop_rate(50); 
    int data_success_flag = 0;
    int data_valid_flag = 0;
    while(ros::ok()) 
    {

        if(ser.available()){ 
            ROS_INFO_STREAM("Reading from serial port\n"); 
            std_msgs::String force_string; 
            string temp_str;
            std::vector<int> force_data;
            geometry_msgs::WrenchStamped wrench_data;
            geometry_msgs::WrenchStamped foream_arm_data;
            geometry_msgs::WrenchStamped upper_arm_data;
            std_msgs::Int32MultiArray read_data;//切割后数据以字符串形式发送
            //force_string.data = ser.read(ser.available()); yuanlai
            /*
            while(1)
            {
                force_string.data = ser.read(2);
                if(force_string.data == "DD"){
                    ROS_INFO_STREAM("DDDDDD\n");
                    break;
                }
            }
            */
            while(1)
            {
                temp_str = ser.read(1);
                if(temp_str == "A"){
                    ROS_INFO_STREAM("AAAAAA\n");
                    break;
                }
            }
            while(1)
            {
                temp_str = ser.read(1);
                if(temp_str == "a")
                {
                    ROS_INFO_STREAM("aaaaa\n");
                    if(force_string.data.length() == 32)
                    {
                        data_success_flag = 1;
                        ROS_INFO_STREAM("DATA SECCESS!\n");
                        if(Is_valid(force_string.data) == 1)
                        {
                            data_valid_flag = 1;
                        }
                        else
                        {
                            data_valid_flag = 0;
                        }
                    }
                    else
                    {
                        data_success_flag = 0;
                        ROS_INFO_STREAM("DATA FAILED!\n");
                    }
                    break;
                }
                else
                {
                    force_string.data = force_string.data + temp_str;
                }
                
            }
            //force_string.data = ser.read(44); 
            ROS_INFO_STREAM("Read: " << force_string.data);
            ROS_INFO_STREAM("size"<<force_string.data.size());
            
            //if(force_string.data.length()==44){                 // 左大臂有48位
            if(data_success_flag == 1 && data_valid_flag == 1)
            {
                ROS_INFO_STREAM("IN THE LOOP HAHAHA\n"); 
                read_data.data.clear();
                //force_string.data = force_string.data.substr(2, force_string.data.length() - 2);           // 截掉数据头
                for (int i = 0; i < 7; i++){
                std::string temp = force_string.data.substr(0,4);

                //flirt force
                force_data.push_back((stoi(temp))>200?(stoi(temp))/1000:0);
                // force_data.push_back((3600-stoi(temp))>1200?(3600-stoi(temp))/1200:0);// flirt
                //force_data.push_back(4095-stoi(temp));//convert to force
                // force_data.push_back(stoi(temp));//split
                read_data.data.push_back(stoi(temp));
                force_string.data = force_string.data.substr(4,force_string.data.size());
                }
                force_data.push_back((stoi(force_string.data))>200?(stoi(force_string.data))/1000:0);
                // force_data.push_back((3600-stoi(force_string.data))>1200?(3600-stoi(force_string.data))/1200:0);
                //force_data.push_back(4095-stoi(force_string.data));
                // force_data.push_back(stoi(force_string.data));
                read_data.data.push_back(stoi(force_string.data));
                //ROS_INFO_STREAM("split"<<force_data[0]<<" "<<force_data[1]<<" "<<force_data[2]<<" "<<force_data[3]<<" "<<force_data[4]<<" "<<force_data[5]<<" "<<force_data[6]<<" "<<force_data[7]<<" "<<force_data[8]<<" "<<force_data[9]<<" "<<force_data[10]<<" "<<force_data[11]<<" "<<force_data[12]<<" "<<force_data[13]<<" "<<force_data[14]<<" "<<force_data[15]);
                //ROS_INFO_STREAM("split"<<force_data[0]<<" "<<force_data[1]<<" "<<force_data[2]<<" "<<force_data[3]<<" "<<force_data[4]<<" "<<force_data[5]<<" "<<force_data[6]<<" "<<force_data[7]<<" "<<force_data[8]<<" "<<force_data[9]<<" "<<force_data[10]<<" "<<force_data[11]<<" "<<force_data[12]<<" "<<force_data[13]<<" "<<force_data[14]<<" "<<force_data[15]);
                //ROS_INFO_STREAM("split"<<force_data[0]<<" "<<force_data[1]<<" "<<force_data[2]<<" "<<force_data[3]<<" "<<force_data[4]<<" "<<force_data[5]<<" "<<force_data[6]<<" "<<force_data[7]<<" "<<force_data[8]<<" "<<force_data[9]);
                ROS_INFO_STREAM("split"<<read_data.data[0]<<" "<<read_data.data[1]<<" "<<read_data.data[2]<<" "<<read_data.data[3]<<" "<<read_data.data[4]<<" "<<read_data.data[5]<<" "<<read_data.data[6]<<" "<<read_data.data[7]);
                //orgnize data

                wrench_data = upper_arm_data;
                read_pub.publish(wrench_data); 
                
                read_data_pub.publish(read_data);
            }
                   
        } 

        //处理ROS的信息，比如订阅消息,并调用回调函数 
        ros::spinOnce(); 
        loop_rate.sleep(); 

    } 
}
